
#include <agv_peripheral_controller/ioLiftDoubleSteer/io_lifter_controller.h>
#include <agv_srvs/srv/output_interface.hpp>

namespace agv_peripheral
{

    CIoLift::CIoLift()
    {
    }

    CIoLift::~CIoLift()
    {
    }

    bool CIoLift::init(ros::ServiceClient &client)
    {
        m_client = client;
        return true;
    }

    // 举升上升
    bool CIoLift::up()
    {
        bool finish = false;
        if (ctrl.size() == 0 || ctrl.size() > 2)
        {
            return finish;
        }
        // 如果到达上限位，停止完成此动作
        if (upTarget())
        {
            finish = stop();
            DEBUG_STREAM_OUT(("CIoLift:up target:key=" << ctrl[0].key
                                                    << ",set stop finish=" << finish);
            return finish;
        }

        // 控制上升输出

        // 控制停止输出
        if (ctrl.size() == 2)
        {
            ctrl[0].value = true;
            ctrl[1].value = false;
        }
        else
        {
            ctrl[0].value = true;
        }
        bool ret = outputIo(ctrl);
        DEBUG_OUT("up:set io out key=" << ctrl[0].key << ",ret=" << ret);
        return finish;
    }

    // 举升下降
    bool CIoLift::down()
    {
        bool finish = false;
        if (ctrl.size() == 0 || ctrl.size() > 2)
        {
            return finish;
        }

        // 如果到达下限位，停止完成此动作
        if (downTarget())
        {
            finish = stop();
            DEBUG_STREAM_OUT(("CIoLift:down target:key=" << ctrl[0].key
                                                      << ",set stop finish=" << finish);
            return finish;
        }

        // 控制停止输出
        if (ctrl.size() == 2)
        {
            ctrl[0].value = false;
            ctrl[1].value = true;
        }
        else
        {
            ctrl[0].value = true;
        }
        bool ret = outputIo(ctrl);
        DEBUG_OUT("down:set io out key=" << ctrl[0].key << ",ret=" << ret);
        return finish;
    }

    // 举升停止
    bool CIoLift::stop()
    {
        bool finish = false;

        if (ctrl.size() == 0 || ctrl.size() > 2)
        {
            return finish;
        }

        // 控制停止输出
        if (ctrl.size() == 2)
        {
            ctrl[0].value = false;
            ctrl[1].value = false;
        }
        else
        {
            ctrl[0].value = false;
        }
        finish = outputIo(ctrl);

        if (finish)
        {
            DEBUG_WARN_OUT("stop lift ok");
        }
        return finish;
    }

    // 货物装填
    bool CIoLift::loadStatus()
    {
        bool load = false;
        // 如果不处在下限位，则一直报有货状态
        bool unload = downTarget() && !upTarget();
        load = !unload;
        return load;
    }

    // 上升到位
    bool CIoLift::upTarget()
    {
        bool ret = inputIo(upLimit);
        if (ret)
        {
            DEBUG_WARN_OUT("upTarget ok");
        }
        return ret;
    }

    // 下降到位
    bool CIoLift::downTarget()
    {
        bool ret = inputIo(downLimit);
        if (ret)
        {
            DEBUG_WARN_OUT("downTarget ok");
        }
        return ret;
    }

    // 控制IO点输出 param1:索引 param2:触发
    bool CIoLift::outputIo(IO_GROUP outIoGroup)
    {
        // 初始化是否完成
        bool ret = false;

        // 客户端是否有效
        if (!m_client.isValid())
        {
            DEBUG_ERROR_OUT("CIoLift:outputinterface. client is not valid");
            return ret;
        }

        if (m_outIoRecord == outIoGroup)
        {
            // DEBUG__OUT("CIoLift:outIoGroup is the same as last time");
            return true;
        }

        agv_srvs::srv::OutputsNew srv;
        srv.request.array = outIoGroup;
        if (m_client.call(srv) && srv.response.state)
        {
            ret = true;
            m_outIoRecord = outIoGroup;
            DEBUG_OUT("CIoLift:control io sucess ;");
        }
        else
        {
            ret = false;
            DEBUG_ERROR_OUT("CIoLift: control io fail;");
        }

        return ret;
    }

    ///////////////////////////////CIoLifterController 实现///////////////////////////////////////////////
    // 默认构造函数
    CIoLifterController::CIoLifterController()
    {
        m_init = false;
    }

    // 析构函数
    CIoLifterController::~CIoLifterController()
    {
        m_subIo.shutdown();
    }

    // 是否初始化完成
    bool CIoLifterController::initFinish()
    {
        if (!m_init)
        {
            DEBUG_ERROR_OUT("IoLifterController init failed");
        }
        return m_init;
    }

    // 初始化函数
    bool CIoLifterController::init()
    {
        // 订阅输入信号话题/readInput
        m_subIoNew = node->create_subscription<agv_msgs::msg::ReadInPutsNew>(
                "/readInputNew", 10,
                std::bind(&CIoLifterController::readInputNewCallBack, this, std::placeholders::_1)
            );

        m_outClient = node->create_client<agv_srvs::srv::OutputsNew>("/outputNew");

        // 初始化io key-名字
        agv_msgs::msg::PairTypeInt io;
        io.value = 0;
        // 左升降销
        io.key = UP_LIMIT_1;
        m_leftIoLift.upLimit.Update(io);
        io.key = DOWN_LIMIT_1;
        m_leftIoLift.downLimit.Update(io);
        io.key = CONTROL_IO_1;
        m_leftIoLift.ctrl.push_back(io);

        // 右升降销
        io.key = UP_LIMIT_2;
        m_rightIoLift.upLimit.Update(io);
        io.key = DOWN_LIMIT_2;
        m_rightIoLift.downLimit.Update(io);
        io.key = CONTROL_IO_2;
        m_rightIoLift.ctrl.push_back(io);

        // 液压举升-如果数量为2 默认第一个升，第二个io为降
        io.key = UP_LIMIT_3;
        m_normalLift.upLimit.Update(io);
        io.key = DOWN_LIMIT_3;
        m_normalLift.downLimit.Update(io);
        io.key = CONTROL_IO_3_UP;
        m_normalLift.ctrl.push_back(io);
        io.key = CONTROL_IO_3_DOWN;
        m_normalLift.ctrl.push_back(io);

        // 举升旋钮开关
        m_switch.resize(SWITCH_COUNT);
        io.key = IO_LIFT_UP_SWITCH;
        m_switch[ioLiftUp].Update(io);
        io.key = IO_LIFT_DOWN_SWITCH;
        m_switch[ioLiftDown].Update(io);
        io.key = LIFT_UP_SWITCH;
        m_switch[normalLiftUp].Update(io);
        io.key = LIFT_DOWN_SWITCH;
        m_switch[normalLiftDown].Update(io);

        // 急停按钮
        io.key = EMG_BUTTON;
        m_emgBtn.Update(io);

        // 货物检测开关
        io.key = DETECTOR;
        m_detector.Update(io);

        m_leftIoLift.init(m_outClient);
        m_rightIoLift.init(m_outClient);
        m_normalLift.init(m_outClient);

        m_init = true;
        return true;
    }

    // 举升上升
    bool CIoLifterController::up()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        // 控制下降输出
        bool ret = true;
        ret &= m_normalLift.up();
        // ret &= m_rearLift.up();
        DEBUG_STREAM_OUT(("IoLifterController:up :"
                       << ",ret=" << ret);

        finish = ret;

        return finish;
    }

    // 举升下降
    bool CIoLifterController::down()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        // 控制下降输出
        bool ret = true;
        ret &= m_normalLift.down();
        DEBUG_STREAM_OUT(("IoLifterController:down :"
                       << ",ret=" << ret);

        finish = ret;

        return finish;
    }

    // 所有举升停止
    bool CIoLifterController::stop()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        bool ret = true;
        ret &= m_normalLift.stop();
        DEBUG_STREAM_OUT(("IoLifterController:stop :"
                       << ",ret=" << ret);
        finish = ret;

        if (finish)
        {
            DEBUG_WARN_OUT("stop lift ok");
        }
        return finish;
    }

    // 急停按下
    bool CIoLifterController::emgPressed()
    {
        bool ret = false;
        if (!initFinish())
        {
            return ret;
        }

        ret = inputIo(m_emgBtn);
        if (ret)
        {
            DEBUG_WARN_OUT("emg preesed");
        }

        return ret;
    }

    // 货物检测
    bool CIoLifterController::loadDetector()
    {
        bool ret = false;
        if (!initFinish())
        {
            return ret;
        }

        ret = inputIo(m_detector);
        // if (ret)
        // {
        //     DEBUG_WARN_OUT("detector preesed");
        // }

        return ret;
    }

    ///////////////////特殊使用需求///////////////////
    // 左右升降升降销
    bool CIoLifterController::upLeftRight()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        bool ret = true;
        ret &= m_leftIoLift.up();
        ret &= m_rightIoLift.up();
        DEBUG_STREAM_OUT(("upLeftRight:up :"
                       << ",ret=" << ret);
        finish = ret;

        if (finish)
        {
            DEBUG_WARN_OUT("upLeftRight finis ok");
        }
        return finish;
    }

    // 左右升降升降销
    bool CIoLifterController::downLeftRight()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        bool ret = true;
        ret &= m_leftIoLift.down();
        ret &= m_rightIoLift.down();
        DEBUG_STREAM_OUT(("downLeftRight:up :"
                       << ",ret=" << ret);
        finish = ret;

        if (finish)
        {
            DEBUG_WARN_OUT("downLeftRight finis ok");
        }
        return finish;
    }

    // 左右升降升降销
    bool CIoLifterController::stopLeftRight()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        bool ret = true;
        ret &= m_leftIoLift.stop();
        ret &= m_rightIoLift.stop();
        // DEBUG_OUT("stopLeftRight:"
        //                << ",ret=" << ret);
        finish = ret;

        if (finish)
        {
            DEBUG_OUT("stopLeftRight finis ok");
        }
        return finish;
    }

    SwitchState CIoLifterController::getSwtichState(SwitchType type)
    {
        SwitchState ret = SwitchState::posMiddle;
        if (m_switch.size() < SWITCH_COUNT)
        {
            DEBUG_ERROR_OUT("getSwtichState failed switch count is less");
            return ret;
        }

        switch (type)
        {
        case SwitchType::ioLiftType:
        {
            if (inputIo(m_switch[ioLiftUp]))
            {
                ret = SwitchState::posLeftUp;
            }
            else if (inputIo(m_switch[ioLiftDown]))
            {
                ret = SwitchState::posRightDown;
            }
        }

        break;
        case SwitchType::normalLiftType:
        {
            if (inputIo(m_switch[normalLiftUp]))
            {
                ret = SwitchState::posLeftUp;
            }
            else if (inputIo(m_switch[normalLiftDown]))
            {
                ret = SwitchState::posRightDown;
            }
        }

        break;
        }

        return ret;
    }

    bool CIoLifterController::manulSwitchIoLift(bool reset)
    {
        bool ret = false;
        static SwitchState lastState1 = SwitchState::posInvalid;
        if (!initFinish())
        {
            return ret;
        }

        SwitchState curState1 = getSwtichState(SwitchType::ioLiftType);
        // #1 当自动切换到手动时，防止举升开关在上下降档位，误动作
        if (reset)
        {
            lastState1 = SwitchState::posInvalid;
            // DEBUG_STREAM_OUT("manulSwitchCtrl:reset");
            return ret;
        }

        // 上次复位过，必须先打回中间，在切换
        if (lastState1 == SwitchState::posInvalid)
        {
            if (curState1 != SwitchState::posMiddle)
            {
                DEBUG_WARN_OUT("reset: curState1!= posMiddle");
                return ret;
            }
            lastState1 = curState1;
            DEBUG_WARN_OUT("reset ok; curState1=posMiddle");
            return ret;
        }

        // 正常动作
        ret = true;
        // 液压举升,中间档外部管控
        switch (curState1)
        {
        case SwitchState::posLeftUp:
            ret &= upLeftRight();
            break;

        case SwitchState::posRightDown:
            ret &= downLeftRight();
            break;
        default:
            break;
        }

        DEBUG_STREAM_OUT("manulSwitchCtrl:lastState1!=curState1: lastState1=" << (int)(lastState1) << ",curState1=" << (int)(curState1)
                                                                             << ",ret=" << ret);
        lastState1 = curState1;
        return curState1 != SwitchState::posMiddle;
    }

    bool CIoLifterController::manulSwitchNormalLift(bool reset)
    {
        bool ret = false;
        static SwitchState lastState2 = SwitchState::posInvalid;
        if (!initFinish())
        {
            return ret;
        }

        // #1 当自动切换到手动时，防止举升开关在上下降档位，误动作
        SwitchState curState2 = getSwtichState(SwitchType::normalLiftType);
        if (reset)
        {
            lastState2 = SwitchState::posInvalid;
            // DEBUG_STREAM_OUT("manulSwitchCtrl:reset");
            return ret;
        }

        // 上次复位过，必须先打回中间，在切换

        if (lastState2 == SwitchState::posInvalid)
        {
            if (curState2 != SwitchState::posMiddle)
            {
                DEBUG_WARN_OUT("reset: curState2!= posMiddle");
                return ret;
            }
            lastState2 = curState2;
            DEBUG_WARN_OUT("reset ok; curState2=posMiddle");
            return ret;
        }

        // 正常动作
        ret = true;
        // 液压举升,中间档外部管控
        switch (curState2)
        {
        case SwitchState::posLeftUp:
            ret &= up();
            break;

        case SwitchState::posRightDown:
            ret &= down();
            break;
        default:
            break;
        }

        DEBUG_STREAM_OUT("manulSwitchCtrl:lastState2!=curState2: lastState2=" << (int)(lastState2) << ",curState2=" << (int)(curState2)
                                                                             << ",ret=" << ret);
        lastState2 = curState2;
        return curState2 != SwitchState::posMiddle;
    }

    // 货物装填
    bool CIoLifterController::loadStatus()
    {
        bool load = false;
        if (!initFinish())
        {
            return load;
        }
        // 特殊的货物检测开关
        // 液压举升不在最低处，防止撞工位，使用高度作为有无货物的其中一种条件
        load = m_normalLift.loadStatus();
        return load;
    }

    // 新更新输入IO对象
    void CIoLifterController::updateInutIoNew(const agv_msgs::msg::ReadInPutsNew &msg, INPUT_IO_TYPE &inputIoObj)
    {
        auto inputIo = inputIoObj.Get();
        std::vector<agv_msgs::msg::PairTypeInt> inputs_msg = msg.array;
        for (auto &data : inputs_msg)
        {
            if (data.key == inputIo.key)
            {
                inputIoObj.Update(data);
                // DEBUG_OUT("updateInutIoNew:inputIo.key=" << inputIo.key << ",data.value=" << data.value);
                break;
            }
        }
    }

    // 获取IO输入信号回调函数 io topic话题频率30hz
    void CIoLifterController::readInputNewCallBack(const agv_msgs::msg::ReadInPutsNew &msg)
    {
        // 获取前、后举升上下限位IO信号 并更新数据的时间戳
        updateInutIoNew(msg, m_leftIoLift.upLimit);
        updateInutIoNew(msg, m_leftIoLift.downLimit);
        updateInutIoNew(msg, m_rightIoLift.upLimit);
        updateInutIoNew(msg, m_rightIoLift.downLimit);
        updateInutIoNew(msg, m_normalLift.upLimit);
        updateInutIoNew(msg, m_normalLift.downLimit);

        // 更新旋钮开关的状态
        for (auto &s : m_switch)
        {
            updateInutIoNew(msg, s);
        }

        // // 获取急停的
        updateInutIoNew(msg, m_emgBtn);
        // 货物检测开关
        updateInutIoNew(msg, m_detector);
    }
}; // end namespace
